///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_EIGEN_H_
#define _BUOLA_MAT_EIGEN_H_

#include <buola/mat.h>
#include <buola/mat/detail/eigen_wrapper.h>

#include <Eigen/Eigenvalues>

namespace buola { namespace mat {

template<typename tDerived>
std::pair<CVec<detail::OScalar<tDerived>>,CMat<detail::OScalar<tDerived>>> eigen_vectors(const CExpr<tDerived> &pExpr)
{
    assert(pExpr.Cols()==pExpr.Rows());
    CVec<detail::OScalar<tDerived>> lAResult(pExpr.Derived().Cols());
    CMat<detail::OScalar<tDerived>> lVResult(pExpr.Derived().Cols(),pExpr.Derived().Rows());
    
    Eigen::SelfAdjointEigenSolver<detail::OEigenMatrixType<decltype(detail::eigen_mappable(pExpr.Derived()))>> lSolver(detail::eigen_map(detail::eigen_mappable(pExpr.Derived())));
    
    detail::eigen_map(lAResult)=lSolver.eigenvalues();
    detail::eigen_map(lVResult)=lSolver.eigenvectors();
    
    return {std::move(lAResult),std::move(lVResult)};
}

///compute eigenvalues of a 3x3 symmetric matrix
template<typename tDerived,OEnableIf<detail::QHasRowsAndCols<tDerived,3>> =0>
CVec<detail::OScalar<tDerived>,3> compute_roots(const CExpr<tDerived> &pExpr)
{
    typedef detail::OScalar<tDerived> TScalar;
    const auto &lE=detail::element_accessible<detail::ACCESS_FAST>(pExpr.Derived());
    
    TScalar lC0=lE(0,0)*lE(1,1)*lE(2,2)
               +TScalar(2)*lE(0,1)*lE(0,2)*lE(1,2)
               -lE(0,0)*sq(lE(1,2))
               -lE(1,1)*sq(lE(0,2))
               -lE(2,2)*sq(lE(0,1));
    TScalar lC1=lE(0,0)*lE(1,1)
               -sq(lE(0,1))
               +lE(0,0)*lE(2,2)
               -sq(lE(0,2))
               +lE(1,1)*lE(2,2)
               -sq(lE(1,2));
    TScalar lC2=lE(0,0)+lE(1,1)+lE(2,2);
    if(abs(lC0)<std::numeric_limits<TScalar>::epsilon())
    {
        TScalar lD=(lC2*lC2-TScalar(4.0)*lC1);
        if(lD<TScalar(0.0))
        {
            msg_info() << lE << "\n";
            msg_info() << lC0 << " " << lC1 << "\n";
            msg_warn() << "no real roots in compute_roots\n";
            lD=0.0;
        }
        
        lD=sqrt(lD);
        
        return {TScalar(0),TScalar(0.5)*(lC2-lD),TScalar(0.5)*(lC2+lD)};
    }
    else
    {
        const TScalar lInv3=TScalar(1.0/3.0);
        const TScalar lSqrt3=sqrt(TScalar(3.0));
        TScalar lC2Over3=lC2*lInv3;
        TScalar lAOver3=(lC1-lC2*lC2Over3)*lInv3;
        if(lAOver3>0) lAOver3=0;
        TScalar lHalfB=TScalar(0.5)*(lC0+lC2Over3*(TScalar(2)*sq(lC2Over3)-lC1));
        TScalar lQ=sq(lHalfB)+lAOver3*lAOver3*lAOver3;
        if(lQ>0) lQ=0;
        
        TScalar lRho=sqrt(-lAOver3);
        TScalar lTheta=TScalar(atan2(sqrt(-lQ),lHalfB))*lInv3;
        TScalar lCosTheta=cos(lTheta);
        TScalar lSinTheta=sin(lTheta);
        CVec<TScalar,3> lR;
        lR[0]=lC2Over3+TScalar(2)*lRho*lCosTheta;
        lR[1]=lC2Over3-lRho*(lCosTheta+lSqrt3*lSinTheta);
        lR[2]=lC2Over3-lRho*(lCosTheta-lSqrt3*lSinTheta);
        sort3(lR[0],lR[1],lR[2]);
        return lR;
    }
}

/*namespace mat*/ } /*namespace buola*/ }

#endif
